#include "quaternion.h"

quaternion::quaternion(void)
{
	w = 0;
	x = y = z = 0;
}

quaternion::quaternion(double d, double a, double b, double c){
	w = d;
	x = a;
	y = b;
	z = c;
}
quaternion::quaternion(double angle, vector3 axis)
{
	double c = cos(angle/2);
	double s = sin(angle/2);
	w = c;
	x = axis.x * s;
	y = axis.y * s;
	z = axis.z * s;
}

quaternion::~quaternion(void)
{
}

double quaternion::length(){
	return sqrt(w*w + x*x + y*y + z*z);
}

quaternion quaternion::normalize(){
	double l = (double)length();
	x /= l;
	y /= l;
	z /= l;
	w /= l;
	return *this;
}

quaternion quaternion::normalized()
{
	double l = (double)length();
	return quaternion(w/l, x/l, y/l, z/l);
}

quaternion quaternion::conjugate(){
	quaternion q(w, -x,-y,-z);
	return q;
}

vector3 quaternion::getVector()
{
	return vector3(x, y, z);
}

quaternion quaternion::operator+ (const quaternion &rhs){
	return quaternion(w + rhs.w, x + rhs.x, y + rhs.y, z + rhs.z);
}

quaternion quaternion::operator* (const quaternion &rhs){
	quaternion q;
	quaternion lhs = *this;
	q.x = lhs.w*rhs.x + lhs.x*rhs.w + lhs.y*rhs.z - lhs.z*rhs.y;
	q.y = lhs.w*rhs.y - lhs.x*rhs.z + lhs.y*rhs.w + lhs.z*rhs.x;
	q.z = lhs.w*rhs.z + lhs.x*rhs.y - lhs.y*rhs.x + lhs.z*rhs.w;
	q.w = lhs.w*rhs.w - lhs.x*rhs.x - lhs.y*rhs.y - lhs.z*rhs.z;
	return q; 
}

quaternion quaternion::operator* (const double &s)
{
	return quaternion(w * s, x * s, y * s, z * s);
}

quaternion quaternion::operator/ (const double &s)
{
	return quaternion(w / s, x / s, y / s, z / s);
}

quaternion& quaternion::operator+= (const quaternion &rhs)
{
		w += rhs.w;
        x += rhs.x;
		y += rhs.y;
		z += rhs.z;
		return *this;
}

quaternion& quaternion::operator-= (const quaternion &rhs)
{
		w += rhs.w;
        x -= rhs.x;
		y -= rhs.y;
		z -= rhs.z;
		return *this;
}

vector3 quaternion::rotate(vector3 vector, quaternion rotation)
{
	vector3 unit = vector.normalized();

	quaternion unitQ = quaternion(0.0f, vector.x, vector.y, vector.z);
	quaternion resultQ = rotation * unitQ;
	resultQ = resultQ * rotation.conjugate();
 
	return (vector3(resultQ.x, resultQ.y, resultQ.z));
}

quaternion quaternion::rotate(quaternion orientation, quaternion rotation)
{
	orientation.normalize();
	quaternion unitQ = orientation;
	quaternion resultQ = rotation * unitQ;
	resultQ = resultQ * rotation.conjugate();
 
	return (resultQ);
}

double* quaternion::toMatrix(bool normalized){
	if(normalized)
	{
		double m[16] = { 1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y, 0,
						2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x, 0,
						2*x*z - 2*w*y, 2*y*z - 2*w*x, 1-2*x*x - 2*y*y, 0,
						0, 0, 0, 1};
		return m;
	} 
	else 
	{
		double m[16] = { w*w + x*x + y*y + z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y, 0,
						2*x*y + 2*w*z, w*w - x*x + y*y - z*z, 2*y*z - 2*w*x, 0,
						2*x*z - 2*w*y, 2*y*z - 2*w*x, w*w - x*x - y*y + z*z, 0,
						0,0,0,1};
		return m;
	}
}